#include "Header.h"
#include <dsp.h>

/**
fractional GyroGain = 0.004261058 * FRACT_214_10 * (1 / F_IMU_LOOP) * 2;
fractional AccelGain = 0.039517822 * FRACT_214_10 * (1 / F_IMU_LOOP);

fractional v3_GyroCorrP[] = {0, 0, 0};
fractional v3_GyroCorrI[] = {0, 0, 0};

fractional KpRollPitch = 0.2 * FRACT_214_10;
fractional KiRollPitch = (1 / 128) * FRACT_214_10;

fractional m3x3_Rotation[] = { FRACT_214_10, 0, 0, 
							   0, FRACT_214_10, 0,
							   0, 0, FRACT_214_10 };	

void UpdateRotationMatrix(void)
{
	fractional v3_GyroRates[3], v3_omegaCorrection[3], v3_omega[3];
	fractional v3_theta[3], m3x3_Update[9], m3x3_Temp[9];

	v3_GyroRates[0] = XRate.value;
	v3_GyroRates[1] = YRate.value;
	v3_GyroRates[2] = ZRate.value;

	VectorAdd(3, &v3_omegaCorrection, &v3_omegaCorrI, &v3_omegaCorrP);
	VectorAdd(3, &v3_omega, &v3_GyroRates, &v3_omegaCorrection);
	VectorScale(3, &v3_theta, &v3_omega, GyroGain); // The GyroGain contains a factor 2 to compensate for th

	m3x3_Update[0] = 1;
	m3x3_Update[1] = -v3_theta[2];
	m3x3_Update[2] = v3_theta[1];
	m3x3_Update[3] = v3_theta[2];
	m3x3_Update[4] = 1;
	m3x3_Update[5] = -v3_theta[0];
	m3x3_Update[6] = -v3_theta[1];
	m3x3_Update[7] = v3_theta[0];
	m3x3_Update[8] = 1;
	
	MatrixMultiply(3, 3, 3, &m3x3_Temp, &m3x3_Rotation, &m3x3_Update);
	MatrixAdd(3, 3, &m3x3_Rotation, &m3x3_Temp, &m3x3_Temp);
}

void RenormalizeRotationMatrix(void)
{
	fractional v3_rowX[3], v3_rowY[3], v3_rowZ[3];
	fractional m3x3_Temp[9];
    // This variable will hold the error
	fractional error;

	error = - VectorDotProduct(3, &m3x3_Rotation[0], &m3x3_Rotation[3]); // The factor 0.5 is built in in the 2.14 format
	VectorScale(3, &m3x3_Temp[0], &m3x3_Rotation[0], error);
	VectorScale(3, &m3x3_Temp[3], &m3x3_Rotation[3], error);
	
}
**/

void UpdateRotationMatrix(void)
{
     // Declare some vectors
	float v3_GyroRates[3], v3_GyroCorrection[3], v3_omega[3];
	float v3_theta[3], m3x3_Update[9];

    // Use the input from the gyroscopes
	v3_GyroRates[0] = XRate.value;
	v3_GyroRates[1] = YRate.value;
	v3_GyroRates[2] = ZRate.value;
	
    // Convert from A/D units to engineering units
	v3_VectorScale(v3_GyroRates, GyroGain, v3_GyroRates);
    // Calculate the correction vector
	v3_VectorAdd(v3_GyroCorrP, v3_GyroCorrI, v3_GyroCorrection);
    // Add the correction vector to the gyroscope data
	v3_VectorAdd(v3_GyroRates, v3_GyroCorrection, v3_omega);
	// 'Integrate' the gyroscope data
	v3_VectorScale(v3_omega, dt, v3_theta);
	
	// Populate the update matrix
	m3x3_Update[0] = 1;
	m3x3_Update[1] = -v3_theta[2];
	m3x3_Update[2] = v3_theta[1];
	m3x3_Update[3] = v3_theta[2];
	m3x3_Update[4] = 1;
	m3x3_Update[5] = -v3_theta[0];
	m3x3_Update[6] = -v3_theta[1];
	m3x3_Update[7] = v3_theta[0];
	m3x3_Update[8] = 1;
	
	// Udate the rotation matrix by multipliying it by the rotation matrix
	m3x3_MatrixMultiply(m3x3_Rotation, m3x3_Update, m3x3_Rotation);
}

void RenormalizeRotationMatrix(void)
{
    // Initialize some vectors
	float v3_rowX[3], v3_rowY[3], v3_rowZ[3];
	float v3_rowXold[3], v3_rowYold[3];
    // This variable will hold the error
	float error;

    // These temporary variables will be used during calculations
	float temp[3];
	float temp2;
	
	// Retrieve the X and Y rows from the rotation matrix	
	v3_GetRowAsVector(&m3x3_Rotation[0], v3_rowXold);
	v3_GetRowAsVector(&m3x3_Rotation[3], v3_rowYold);
	
	// Calculate the how much the two rows are rotated 
	// towards eachother
	v3_VectorDotProduct(v3_rowXold, v3_rowYold, &error);

    // Use the temporary variable to hold the value needed
    // to correct the X row
	v3_VectorScale(v3_rowYold, -0.5 * error, temp);
	v3_VectorAdd(v3_rowXold, temp, v3_rowX);

    // Use the temporary variable to hold the value needed
    // to correct the Y row
	v3_VectorScale(v3_rowXold, -0.5 * error, temp);
	v3_VectorAdd(v3_rowYold, temp, v3_rowY);

    // Using the orthogonal X & Y rows, we can calculate 
    // the orthogonal Z row
	v3_VectorCrossProduct(v3_rowX, v3_rowY, v3_rowZ);

    // Make sure the length of the X row is 1
	v3_VectorDotProduct(v3_rowX, v3_rowX, &temp2);
	temp2 = 0.5 * (3 - temp2);
	v3_VectorScale(v3_rowX, temp2, v3_rowX);
	
	// Make sure the length of the X row is 1
	v3_VectorDotProduct(v3_rowY, v3_rowY, &temp2);
	temp2 = 0.5 * (3 - temp2);
	v3_VectorScale(v3_rowY, temp2, v3_rowY);
	
    // Make sure the length of the X row is 1
	v3_VectorDotProduct(v3_rowZ, v3_rowZ, &temp2);
	temp2 = 0.5 * (3 - temp2);
	v3_VectorScale(v3_rowZ, temp2, v3_rowZ);

    // Update the rotation matrix to contain the
    // new, corrected X, Y & Z rows
	m3x3_MatrixSetRow(&m3x3_Rotation[0], v3_rowX);	
	m3x3_MatrixSetRow(&m3x3_Rotation[3], v3_rowY);
	m3x3_MatrixSetRow(&m3x3_Rotation[6], v3_rowZ);
}

void CancelDrift(void)
{
    // Declare some vectors
	float v3_gRef[3], v3_rowZ[3], v3_errorRP[3], v3_errorYaw[3], v3_temp[3];

    // Use the input from the accelerometers
	v3_gRef[0] = XAccel.value;
	v3_gRef[1] = YAccel.value;
	v3_gRef[2] = ZAccel.value;	

    // Convert from A/D units to engineering units
	v3_VectorScale(v3_gRef, AccelGain, v3_gRef);
    // Retrieve the Z row from the rotation matrix
	v3_GetRowAsVector(&m3x3_Rotation[6], v3_rowZ);
	// Calculate the error in the roll-pitch plane
	v3_VectorCrossProduct(v3_rowZ, v3_gRef, v3_errorRP);

     // Calculate the correction for the roll-pitch plane
	v3_VectorScale(v3_errorRP, KpRollPitch, v3_GyroCorrP);
	v3_VectorScale(v3_errorRP, KiRollPitch, v3_temp);
	v3_VectorAdd(v3_temp, v3_GyroCorrI, v3_GyroCorrI);
}
